 % 激光雷达参数
laser_range = 10; % 激光雷达的检测范围
scan_angle = linspace(-pi/2, pi/2, 100); % 扫描角度范围
scan_resolution = 0.01; % 扫描分辨率

% 障碍物模型，这里我们使用一个简单的圆形障碍物
obstacle_radius = 2;
obstacle_center = [5, 0];

% 移动模型参数
num_steps = 100; % 移动步数
step_size = 0.5; % 每步移动的距离
initial_position = [0, 0]; % 初始位置

% 初始化
shortest_distance = inf;
shortest_angle = 0;
figure; % 创建图形窗口
hold on; % 保持图形，以便在同一窗口中绘制

% 移动激光雷达并模拟扫描
for step = 1:num_steps
    current_position = initial_position + step * step_size * [1, 0]; % 沿x轴正方向移动

    % 清除之前的图形
    clf;
    scatter(current_position(1), current_position(2), 'r', 'filled'); % 绘制当前位置
    title('LIDAR Simulation');
    xlabel('X Position');
    ylabel('Y Position');

    % 模拟激光雷达扫描
    for i = 1:length(scan_angle)
        angle = scan_angle(i);
        laser_vector = [cos(angle), sin(angle)]; % 激光束方向向量
        distance = laser_range; % 初始假设最大距离

        % 检测障碍物
        for obstacle in obstacles % 假设obstacles是一个障碍物数组
            if norm(obstacle_center - current_position - distance * laser_vector) < obstacle_radius
                distance = norm(obstacle_center - current_position);
                break;
            end
        end

        % 更新最短距离和方向
        if distance < shortest_distance
            shortest_distance = distance;
            shortest_angle = angle;
        end

        % 绘制激光束
        quiver(current_position(1), current_position(2), ...
               laser_vector(1) * distance, laser_vector(2) * distance, ...
               'AutoScale', 'off', 'LineWidth', 2);
    end

    % 绘制最短距离和方向
    quiver(current_position(1), current_position(2), ...
           cos(shortest_angle) * shortest_distance, sin(shortest_angle) * shortest_distance, ...
           'LineWidth', 4, 'Color', 'm');

    % 显示最短距离和角度
    text(current_position(1), current_position(2), ...
         ['d = ', num2str(shortest_distance), ' m, \u03b8 = ', num2str(degrees(shortest_angle)), '°'], ...
         'FontSize', 14, 'Color', 'b');

    drawnow; % 立即更新图形
    pause(0.1); % 稍微暂停，以便观察
end